//==================================================================
//  Copyright (C) 2006  Davide Pasca
//
//  This library is free software; you can redistribute it and/or
//  modify it under the terms of the GNU Lesser General Public
//  License as published by the Free Software Foundation; either
//  version 2.1 of the License, or (at your option) any later version.
//
//  This library is distributed in the hope that it will be useful,
//  but WITHOUT ANY WARRANTY; without even the implied warranty of
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
//  Lesser General Public License for more details.
//
//  You should have received a copy of the GNU Lesser General Public
//  License along with this library; if not, write to the Free Software
//  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
//==================================================================
//==
//==
//==
//==
//==================================================================

#include <string.h>
#include <malloc.h>
#include "videonr.h"

//==================================================================
#define LOW_SIZE	4

//==================================================================
// - based on the fact that blue and red can be interchanged !!!
//   because we swap them
static void lowpass_mini_rgb( const u_char *sp, int w, int h, u_char *dp )
{
int				x, y, yy, pitch, smod;
const u_char	*sp2, *sp3;
u_char			*dp2;
u_int			val1;

	pitch = w * 3;
	smod = pitch - 3*4;

	for (y=0; y < h; y += 4)
	{
		sp2 = sp; sp += pitch*4;
		dp2 = dp; dp += w/4;
		for (x=0; x < w; x += 4)
		{
			val1 = 0;
			sp3 = sp2;
			for (yy=0; yy < LOW_SIZE; ++yy, sp3 += smod)
			{
				val1 += (int)sp3[ 0] + (int)sp3[ 1]*2 + (int)sp3[ 2];
				val1 += (int)sp3[ 3] + (int)sp3[ 4]*2 + (int)sp3[ 5];
				val1 += (int)sp3[ 6] + (int)sp3[ 7]*2 + (int)sp3[ 8];
				val1 += (int)sp3[ 9] + (int)sp3[10]*2 + (int)sp3[11];

				sp3 += 3*4;
			}
			dp2[0] = val1 / (LOW_SIZE*LOW_SIZE*4);

			sp2 += 3*4;
			dp2 += 1;
		}
	}
}

//==================================================================
#define LOW_SIZE	4

//==================================================================
static void lowpass_mini_diff_1( u_char *dp, const u_char *a, const u_char *b, int w, int h )
{
int				x, y, pitch;
const u_char	*a2, *a3;
const u_char	*b2, *b3;
u_char			*dp2;
u_int			val_a;
u_int			val_b;
int				diff;

	pitch = w;

	for (y=0; y < h; y += 4)
	{
		a2 = a; a += pitch * 4;
		b2 = b; b += pitch * 4;
		dp2 = dp; dp += w/4;
		for (x=0; x < w; x += 4)
		{
			a3 = a2; a2 += 4;
			b3 = b2; b2 += 4;

			val_a  = (int)a3[0] + (int)a3[1] + (int)a3[2] + (int)a3[3]; a3 += pitch;
			val_a += (int)a3[0] + (int)a3[1] + (int)a3[2] + (int)a3[3]; a3 += pitch;
			val_a += (int)a3[0] + (int)a3[1] + (int)a3[2] + (int)a3[3]; a3 += pitch;
			val_a += (int)a3[0] + (int)a3[1] + (int)a3[2] + (int)a3[3];

			val_b  = (int)b3[0] + (int)b3[1] + (int)b3[2] + (int)b3[3]; b3 += pitch;
			val_b += (int)b3[0] + (int)b3[1] + (int)b3[2] + (int)b3[3]; b3 += pitch;
			val_b += (int)b3[0] + (int)b3[1] + (int)b3[2] + (int)b3[3]; b3 += pitch;
			val_b += (int)b3[0] + (int)b3[1] + (int)b3[2] + (int)b3[3];

			diff = val_a - val_b;
			//diff = diff ^ (diff >> 31);
			if ( diff < 0 )	diff = -diff;

			dp2[0] = diff / (LOW_SIZE*LOW_SIZE);

			dp2 += 1;
		}
	}
}

//==================================================================
static void expand_mini_diff_1( u_char *desp, const u_char *srcp, int w, int h )
{
int				i, j;
u_char			*desp2;
const u_char	*srcp2;

	for (i=1; i < h-1; ++i)
	{
		desp2 = desp + i * w;
		srcp2 = srcp + i * w;
		for (j=1; j < w-1; ++j)
		{
			desp2 += 1;
			srcp2 += 1;
			desp2[0] = srcp2[-1-w] | srcp2[0-w] | srcp2[1-w] |
					   srcp2[-1  ] | srcp2[0  ] | srcp2[1  ] |
					   srcp2[-1+w] | srcp2[0+w] | srcp2[1+w];
		}
	}

	for (i=0; i < h; i += h-1)
	{
		for (j=0; j < w; j += 1)
		{
		u_char	t;
		int		x, y;

			t = 0;
			for (y=i-1; y <= i+1; ++y)
			{
				if ( y >= 0 && y < h )
				{
					for (x=j-1; x <= j+1; ++x)
					{
						if ( x >= 0 && x < w )
						{
							t |= srcp[ y * w + x ];
						}
					}
				}
			}
			desp[ i * w + j ] = t;
		}
	}

	for (i=0; i < h; i += 1)
	{
		for (j=0; j < w; j += w-1)
		{
		u_char	t;
		int		x, y;

			t = 0;
			for (y=i-1; y <= i+1; ++y)
			{
				if ( y >= 0 && y < h )
				{
					for (x=j-1; x <= j+1; ++x)
					{
						if ( x >= 0 && x < w )
						{
							t |= srcp[ y * w + x ];
						}
					}
				}
			}
			desp[ i * w + j ] = t;
		}
	}
}

//======================================================
static float	_tw[16*9+1];
static int		_mt[16][256];
static int		*_iww;

//======================================================
static void griff_prepare_table(void)
{
int			i,k;
static int	iwww[255*2+1];
static int	K=100;

	for (k=0; k<16; k++)
		for (i=0; i<256; i++)
			_mt[k][i]= k*i;

	_iww = iwww+255;
	for (i=-255; i<=255; i++)
		_iww[i] = K*15 / (K+i*i);

	for (i=1; i<=16*9; i++)
		_tw[i] = (float)(1.0/i);
}

//==================================================================
static _inline long float2int( float d )
{
	/*
long	a;

	__asm {
		fld DWORD PTR [d]
		fistp DWORD PTR [a]
	}
	return a;
	*/
	double dtemp = (((65536.0 * 65536.0 * 16) + (65536.0*.5)) * 65536.0) + d;
	return (*(long *)&dtemp) - 0x80000000;
}

//======================================================
static void griff_4x4( const u_char *sp, u_char *dp, int pitch, int w, int h, int bx, int by )
{
int				x, y, p, p2;
int				yy, xx;
int				t1, t2, t3, n;
const u_char	*sp2;
int				need_clip;
int				*WTX;
float			rt;

	p = pitch;
	p2 = pitch * 2;
	for (y=by; y < by+4; ++y)
	{
		need_clip = (y-1 < 0 || y+1 >= h) || (bx-1 < 0 || bx+3+1 >= w);

		if ( !need_clip )
		{
			for (x=bx; x < bx+4; ++x)
			{
				// t  = somma valori per blur
				// n  = somma dei pesi
				t1 = t2 = t3 = n = 0;
				WTX = _iww-sp[1];

				sp2 = sp - pitch*1;
				for (yy=-1; yy <= 1; ++yy, sp2 += pitch)
				{
				int		w1, w2, w3;
				int		*m1, *m2, *m3;

					w1 = WTX[sp2[-2]];
					w2 = WTX[sp2[1]];
					w3 = WTX[sp2[4]];
					m1 = _mt[w1];
					m2 = _mt[w2];
					m3 = _mt[w3];
					t1 += m1[sp2[-3]] + m2[sp2[0]] + m3[sp2[3]];
					t2 += m1[sp2[-2]] + m2[sp2[1]] + m3[sp2[4]];
					t3 += m1[sp2[-1]] + m2[sp2[2]] + m3[sp2[5]];
					n  += w1+w2+w3;
				}
				rt = _tw[n];
				dp[0] = float2int(t3*rt);
				dp[1] = float2int(t2*rt);
				dp[2] = float2int(t1*rt);

				sp += 3;
				dp += 3;
			}
		}
		else
		{
			for (x=bx; x < bx+4; ++x)
			{
				// t  = somma valori per blur
				// n  = somma dei pesi
				t1 = t2 = t3 = n = 0;
				WTX = _iww-sp[1];

				sp2 = sp - pitch*1;
				for (yy=-1; yy <= 1; ++yy, sp2 += pitch)
				{
				int				w1;
				int				*m1;
				const u_char	*sp3;

					if ( yy+y < 0 || yy+y >= h )
						continue;

					sp3 = sp2 - 1*3;
					for (xx=-1; xx <= 1; ++xx, sp3 += 3)
					{
						if ( xx+x < 0 || xx+x >= w )
							continue;

						w1 = WTX[sp3[1]];
						m1 = _mt[w1];

						t1 += m1[sp3[0]];
						t2 += m1[sp3[1]];
						t3 += m1[sp3[2]];
						n  += w1;
					}
				}
				rt = _tw[n];
				dp[0] = float2int(t3*rt);
				dp[1] = float2int(t2*rt);
				dp[2] = float2int(t1*rt);

				sp += 3;
				dp += 3;
			}
		}

		sp += pitch - 4*3;
		dp += pitch - 4*3;
	}
}

//======================================================
static void griff_4x4_1( const u_char *sp, u_char *dp, int pitch, int w, int h, int bx, int by )
{
int				x, y, p, p2;
int				yy, xx;
int				t1, n;
const u_char	*sp2;
int				need_clip;
int				*WTX;
float			rt;

	p = pitch;
	p2 = pitch * 2;
	for (y=by; y < by+4; ++y)
	{
		need_clip = (y-1 < 0 || y+1 >= h) || (bx-1 < 0 || bx+3+1 >= w);

		if ( !need_clip )
		{
			for (x=bx; x < bx+4; ++x)
			{
				// t  = somma valori per blur
				// n  = somma dei pesi
				t1 = n = 0;
				WTX = _iww-sp[0];

				sp2 = sp - pitch*1;
				for (yy=-1; yy <= 1; ++yy, sp2 += pitch)
				{
				int		w1, w2, w3;
				int		*m1, *m2, *m3;

					w1 = WTX[sp2[-1]];
					w2 = WTX[sp2[ 0]];
					w3 = WTX[sp2[ 1]];
					m1 = _mt[w1];
					m2 = _mt[w2];
					m3 = _mt[w3];
					t1 += m1[sp2[-1]] + m2[sp2[0]] + m3[sp2[1]];
					n  += w1+w2+w3;
				}
				rt = _tw[n];
				dp[0] = float2int(t1*rt);

				sp += 1;
				dp += 1;
			}
		}
		else
		{
			for (x=bx; x < bx+4; ++x)
			{
				// t  = somma valori per blur
				// n  = somma dei pesi
				t1 = n = 0;
				WTX = _iww-sp[0];

				sp2 = sp - pitch*1;
				for (yy=-1; yy <= 1; ++yy, sp2 += pitch)
				{
				int	w1;
				int	*m1;

					if ( yy+y < 0 || yy+y >= h )
						continue;

					for (xx=-1; xx <= 1; ++xx)
					{
						if ( xx+x < 0 || xx+x >= w )
							continue;

						w1 = WTX[sp2[xx]];
						m1 = _mt[w1];

						t1 += m1[sp2[xx]];
						n  += w1;
					}
				}
				rt = _tw[n];
				dp[0] = float2int(t1*rt);

				sp += 1;
				dp += 1;
			}
		}

		sp += pitch - 4*1;
		dp += pitch - 4*1;
	}
}

//==================================================================
int videonr_spacetime1( u_char *desp, const u_char *oldsrcp, const u_char *newsrcp, int w, int h )
{
const u_char	*newsrcp2 = newsrcp;
u_char			*desp2, *desp3;
const u_char	*bp;
u_char			*tbp, *tb2p;
int				t, mw;

int				x, y;
int				pitch;
static int		ow, oh;
u_char			*new_minimapp;
u_char			*cur_minimapp;
u_char			*diff_minimapp;
u_char			*diff2_minimapp;
u_char			*diffp;
const u_char	*oldsrcp2;

/*
	for (y=0; y < h; ++y)
	{
		for (x=0; x < w; ++x)
		{
			desp[2] = newsrcp[0];
			desp[1] = newsrcp[1];
			desp[0] = newsrcp[2];
			desp += 3;
			newsrcp += 3;
		}
	}
	return 0;
*/

	if NOT( new_minimapp = (u_char *)_alloca( w/4 * h/4 ) )
	{
		PASSERT( new_minimapp != NULL );
		return -1;
	}
	if NOT( cur_minimapp = (u_char *)_alloca( w/4 * h/4 ) )
	{
		PASSERT( cur_minimapp != NULL );
		return -1;
	}
	if NOT( diff_minimapp = (u_char *)_alloca( w/4 * h/4 ) )
	{
		PASSERT( diff_minimapp != NULL );
		return -1;
	}

	mw = w/4;

	bp = oldsrcp;

	// - create the two minimaps to compare
	// - based on the fact that blue and red can be interchanged !!!
	//   because we swap them
	lowpass_mini_rgb( newsrcp, w, h, new_minimapp );
	lowpass_mini_rgb( oldsrcp, w, h, cur_minimapp );

	// create the difference values beteen the maps
	tbp = new_minimapp;
	tb2p = cur_minimapp;	
	diffp = diff_minimapp;
	for (y=0; y < h/4; ++y)
	{
		for (x=0; x < w/4; ++x, ++tbp, ++tb2p)
		{
			t = (int)tbp[0] - tb2p[0];
			if ( t < 0 ) t = -t;
			*diffp++ = t;
		}
	}

	// expand the differences in the surrounding areas
	diff2_minimapp = cur_minimapp;
	memcpy( diff2_minimapp, diff_minimapp, w*h/16 );
	tbp = diff_minimapp + 1 + mw;
	tb2p = diff2_minimapp + 1 + mw;
	for (y=1; y < h/4-1; ++y)
	{
		for (x=1; x < mw-1; ++x, ++tbp, ++tb2p)
		{
			tbp[0] = tb2p[-1-mw] | tb2p[0-mw] | tb2p[1-mw] |
					 tb2p[-1   ] | tb2p[0   ] | tb2p[1   ] |
					 tb2p[-1+mw] | tb2p[0+mw] | tb2p[1+mw];
		}
		tbp += 2;
		tb2p += 2;
	}

	pitch = w * 3;

	// make sure the table for griff filter is ready
	griff_prepare_table();

//	prof_start(1);

	newsrcp2 = newsrcp;
	oldsrcp2 = oldsrcp;
	desp2 = desp;
	for (y=0; y < h; y += 4)
	{
	int				yy, xx;
	const u_char	*newsrcp3;
	const u_char	*oldsrcp3;

		tbp = diff_minimapp + y/4 * (w/4);
		for (x=0; x < w; x += 4)
		{
			newsrcp3 = newsrcp2 + x * 3;
			oldsrcp3 = oldsrcp2 + x * 3;
			desp3 = desp2 + x * 3;

			t = (int)tbp[x >> 2];
			if ( t >= 10 )
			{
				griff_4x4( newsrcp3, desp3, pitch, w, h, x, y );
				/*for (yy=4; yy; --yy)
				{
					for (xx=4; xx; --xx)
					{
						bp2[0] = newsrcp3[2];
						bp2[1] = newsrcp3[1];
						bp2[2] = newsrcp3[0];
						bp2 += 3;
						newsrcp3 += 3;
					}
					bp2 += pitch - 3*4;
					newsrcp3 += pitch - 3*4;
				}*/
			}
			else
			if ( t >= 7 )
			{
				for (yy=4; yy; --yy)
				{
					for (xx=4; xx; --xx)
					{
						desp3[0] = ((int)oldsrcp3[0]*1 +	(int)newsrcp3[2]) / 2;
						desp3[1] = ((int)oldsrcp3[1]*1 +	(int)newsrcp3[1]) / 2;
						desp3[2] = ((int)oldsrcp3[2]*1 +	(int)newsrcp3[0]) / 2;
						desp3 += 3;
						oldsrcp3 += 3;
						newsrcp3 += 3;
					}
					newsrcp3 += pitch - 3*4;
					oldsrcp3 += pitch - 3*4;
					desp3 += pitch - 3*4;
				}
			}
			else
			{
				for (yy=4; yy; --yy)
				{
					for (xx=4; xx; --xx)
					{
						desp3[0] = ((int)oldsrcp3[0]*3 +	(int)newsrcp3[2]) / 4;
						desp3[1] = ((int)oldsrcp3[1]*3 +	(int)newsrcp3[1]) / 4;
						desp3[2] = ((int)oldsrcp3[2]*3 +	(int)newsrcp3[0]) / 4;
						desp3 += 3;
						oldsrcp3 += 3;
						newsrcp3 += 3;
					}
					newsrcp3 += pitch - 3*4;
					oldsrcp3 += pitch - 3*4;
					desp3 += pitch - 3*4;
				}
			}
		}
		newsrcp2 += pitch * 4;
		oldsrcp2 += pitch * 4;
		desp2 += pitch * 4;
	}
//	prof_stop();

	return 0;
}

//==================================================================
static int spacetime1_y( u_char *desp, const u_char *oldsrcp, const u_char *newsrcp, int w, int h )
{
const u_char	*newsrcp2 = newsrcp;
u_char			*desp2, *desp3;
u_char			*tbp;
int				t, mw;

int				x, y;
int				pitch;
static int		ow, oh;
u_char			*tmp_diff_minimapp;
u_char			*diff_minimapp;
const u_char	*oldsrcp2;

	//memcpy( desp, newsrcp, w * h );
	//return 0;

	if NOT( tmp_diff_minimapp = (u_char *)_alloca( w/4 * h/4 ) )
	{
		PASSERT( tmp_diff_minimapp != NULL );
		return -1;
	}
	if NOT( diff_minimapp = (u_char *)_alloca( w/4 * h/4 ) )
	{
		PASSERT( diff_minimapp != NULL );
		return -1;
	}

	// create the diff of two minimaps
	lowpass_mini_diff_1( tmp_diff_minimapp, oldsrcp, newsrcp, w, h );
	// expand the differences in the surrounding areas
	expand_mini_diff_1( diff_minimapp, tmp_diff_minimapp, w/4, h/4 );

	pitch = w;

	// make sure the table for griff filter is ready
	griff_prepare_table();

//	prof_start(1);

	mw = w/4;
	newsrcp2 = newsrcp;
	oldsrcp2 = oldsrcp;
	desp2 = desp;
	for (y=0; y < h; y += 4)
	{
	int				yy, xx;
	const u_char	*newsrcp3;
	const u_char	*oldsrcp3;

		tbp = diff_minimapp + y/4 * (w/4);
		for (x=0; x < w; x += 4)
		{
			newsrcp3 = newsrcp2 + x;
			oldsrcp3 = oldsrcp2 + x;
			desp3 = desp2 + x;

			t = (int)tbp[x >> 2];
			if ( t >= 10 )
			{
				griff_4x4_1( newsrcp3, desp3, pitch, w, h, x, y );
				/*for (yy=4; yy; --yy)
				{
					for (xx=4; xx; --xx)
					{
						desp3[0] = newsrcp3[0];
						desp3 += 1;
						newsrcp3 += 1;
					}
					desp3 += pitch - 1*4;
					newsrcp3 += pitch - 1*4;
				}*/
			}
			else
			if ( t >= 7 )
			{
				for (yy=4; yy; --yy)
				{
					for (xx=4; xx; --xx)
					{
						desp3[0] = ((int)oldsrcp3[0] + (int)newsrcp3[0]) / 2;
						desp3 += 1;
						oldsrcp3 += 1;
						newsrcp3 += 1;
					}
					newsrcp3 += pitch - 1*4;
					oldsrcp3 += pitch - 1*4;
					desp3 += pitch - 1*4;
				}
			}
			else
			{
				for (yy=4; yy; --yy)
				{
					for (xx=4; xx; --xx)
					{
						desp3[0] = ((int)oldsrcp3[0]*3 + (int)newsrcp3[0]) / 4;
						desp3 += 1;
						oldsrcp3 += 1;
						newsrcp3 += 1;
					}
					newsrcp3 += pitch - 1*4;
					oldsrcp3 += pitch - 1*4;
					desp3 += pitch - 1*4;
				}
			}
		}
		newsrcp2 += pitch * 4;
		oldsrcp2 += pitch * 4;
		desp2 += pitch * 4;
	}
//	prof_stop();

	return 0;
}

//==================================================================
int videonr_spacetime1_yuv( u_char *y_newp, const u_char *y_oldp, int y_w, int y_h,
							u_char *u_newp, const u_char *u_oldp, int u_w, int u_h,
							u_char *v_newp, const u_char *v_oldp, int v_w, int v_h )
{
int	err;

	err  = spacetime1_y( y_newp, y_oldp, y_newp, y_w, y_h );
	err |= spacetime1_y( u_newp, u_oldp, u_newp, u_w, u_h );
	err |= spacetime1_y( v_newp, v_oldp, v_newp, v_w, v_h );

	return err;
}
